Rotation¶

Euler angles¶

Extrinsic Rotation¶

Right multiply matrix

In [1]:
import sympy
from sympy.vector import AxisOrienter, CoordSys3D
x,y,theta = sympy.symbols("x y theta")
N = CoordSys3D("N")
M = AxisOrienter(theta, N.k).rotation_matrix(N)
M
Out[1]:
$\displaystyle \left[\begin{matrix}\cos{\left(\theta \right)} & \sin{\left(\theta \right)} & 0\\- \sin{\left(\theta \right)} & \cos{\left(\theta \right)} & 0\\0 & 0 & 1\end{matrix}\right]$
In [2]:
M.inv()
Out[2]:
$\displaystyle \left[\begin{matrix}- \frac{\sin^{2}{\left(\theta \right)}}{\cos{\left(\theta \right)}} + \frac{1}{\cos{\left(\theta \right)}} & - \sin{\left(\theta \right)} & 0\\\sin{\left(\theta \right)} & \cos{\left(\theta \right)} & 0\\0 & 0 & 1\end{matrix}\right]$
In [3]:
v = sympy.Matrix([x,y,0]).T * M.subs(theta, sympy.pi/2)
v
Out[3]:
$\displaystyle \left[\begin{matrix}- y & x & 0\end{matrix}\right]$
In [4]:
import py3d
py3d.Transform.from_euler("xyz", [0.4, -0.2, 0])

Welcome to py3d world, please visit https://tumiz.github.io/scenario/ for more information
Out[4]:
Transform([[ 0.98006658,  0.        ,  0.19866933,  0.        ],
           [-0.07736548,  0.92106099,  0.3816559 ,  0.        ],
           [-0.18298657, -0.38941834,  0.9027011 ,  0.        ],
           [ 0.        ,  0.        ,  0.        ,  1.        ]])

Intrinsic Rotation¶

In [5]:
import py3d
py3d.Transform.from_euler("XYZ", [0.4, -0.2, 0])
Out[5]:
Transform([[ 0.98006658, -0.07736548,  0.18298657,  0.        ],
           [ 0.        ,  0.92106099,  0.38941834,  0.        ],
           [-0.19866933, -0.3816559 ,  0.9027011 ,  0.        ],
           [ 0.        ,  0.        ,  0.        ,  1.        ]])
In [6]:
import py3d
py3d.Transform.from_rpy([1.3,1.0,0]) @ py3d.Transform.from_rpy([0.9,.0,0])
Out[6]:
Transform([[ 0.54030231,  0.68032627,  0.49520661,  0.        ],
           [ 0.        , -0.58850112,  0.8084964 ,  0.        ],
           [ 0.84147098, -0.43683247, -0.31796851,  0.        ],
           [ 0.        ,  0.        ,  0.        ,  1.        ]])

Apply a intrinsic rotation to a point

In [8]:
import py3d
v=py3d.Viewer()
p=py3d.Vector3([1,3,4])
v.label("P",p.tolist())
v.render(p.as_point(py3d.Color(g=1)),py3d.Utils.Grid())
t = py3d.Transform.from_euler("XYZ", [0,0,1])
p1 = p @ t
a = py3d.Utils.Axis()
a.vertex @= t
v.label("P1",p1.tolist())
v.render(p1.as_point(py3d.Color(r=1)), a)
v.show()
Out[8]:
py3d

Difference between extrinsic rotation and intrinsic rotation¶

In [9]:
import py3d
v = py3d.Viewer()
e_car = py3d.Utils.Car()
e_car.color = py3d.Color(g=1)
e_car.vertex @= py3d.Transform.from_euler("xyz", [0.4, -0.2, 0])
v.label("Extrincs", [4,1,3], color="green")
i_car = py3d.Utils.Car()
i_car.color = py3d.Color(r=1)
i_car.vertex @= py3d.Transform.from_euler("XYZ", [0.4, -0.2, 0])
v.label("Intrincs", [4,-1,3], color="red")
v.render(e_car, i_car, py3d.Utils.Grid())
v.show()
Out[9]:
py3d

RPY¶

In py3d, rpy represents roll, pitch and yaw, it is kind of intrinsic rotation

In [10]:
from PIL import Image
import numpy
import py3d
img = numpy.array(Image.open("py3d/doc/20220917214012.jpg"))
hw = img.shape[0]//2
hh = img.shape[1]//2
point = py3d.Vector3.grid(range(-hw, hw), range(-hh, hh))[::3, ::3]*0.01
point = point.as_point()
point.color = py3d.Vector3(img/255).H[::3, ::3]
point.vertex @= py3d.Transform.from_rpy([0, 0, -py3d.pi/2])
point.render()
Out[10]:
py3d

Axis angle¶

Axis angle rotation matrix¶

In [11]:
import sympy
x,y,z,theta=sympy.symbols("x y z theta")
from sympy.vector import AxisOrienter, CoordSys3D
N=CoordSys3D('N')
expr=AxisOrienter(theta,x*N.i+y*N.j+z*N.k).rotation_matrix(N)
expr.subs(x**2+y**2+z**2,1)
Out[11]:
$\displaystyle \left[\begin{matrix}x^{2} + \left(1 - x^{2}\right) \cos{\left(\theta \right)} & - x y \cos{\left(\theta \right)} + x y + z \sin{\left(\theta \right)} & - x z \cos{\left(\theta \right)} + x z - y \sin{\left(\theta \right)}\\- x y \cos{\left(\theta \right)} + x y - z \sin{\left(\theta \right)} & y^{2} + \left(1 - y^{2}\right) \cos{\left(\theta \right)} & x \sin{\left(\theta \right)} - y z \cos{\left(\theta \right)} + y z\\- x z \cos{\left(\theta \right)} + x z + y \sin{\left(\theta \right)} & - x \sin{\left(\theta \right)} - y z \cos{\left(\theta \right)} + y z & z^{2} + \left(1 - z^{2}\right) \cos{\left(\theta \right)}\end{matrix}\right]$

Axis angles¶

In [12]:
import py3d
py3d.Transform.from_angle_axis([2, 1], [0, 1, 0])
Out[12]:
Transform([[[-0.41614684,  0.        , -0.90929743,  0.        ],
            [ 0.        ,  1.        ,  0.        ,  0.        ],
            [ 0.90929743,  0.        , -0.41614684,  0.        ],
            [ 0.        ,  0.        ,  0.        ,  1.        ]],

           [[ 0.54030231,  0.        , -0.84147098,  0.        ],
            [ 0.        ,  1.        ,  0.        ,  0.        ],
            [ 0.84147098,  0.        ,  0.54030231,  0.        ],
            [ 0.        ,  0.        ,  0.        ,  1.        ]]])

Order of rotation and translation

In [13]:
import sympy
r=sympy.symbols("r:9")
dr=sympy.symbols("dr:9")
t=sympy.symbols("t:3")
R=sympy.Matrix([
    [r[0],r[1],r[2],0],
    [r[3],r[4],r[5],0],
    [r[6],r[7],r[8],0],
    [0,0,0,1]
])
T=sympy.Matrix([
    [1,0,0,0],
    [0,1,0,0],
    [0,0,1,0],
    [t[0],t[1],t[2],1]
])
R*T
Out[13]:
$\displaystyle \left[\begin{matrix}r_{0} & r_{1} & r_{2} & 0\\r_{3} & r_{4} & r_{5} & 0\\r_{6} & r_{7} & r_{8} & 0\\t_{0} & t_{1} & t_{2} & 1\end{matrix}\right]$

Quaternion¶

In py3d, quaternion is in scalar-last format

In [14]:
import py3d
q = [
    [0, 0, 0, 1],
    [0, 0, 1, 0],
    [0, 1, 0, 0],
    [1, 0, 0, 0]
]
py3d.Transform.from_quaternion(q)
Out[14]:
Transform([[[ 1.,  0.,  0.,  0.],
            [ 0.,  1.,  0.,  0.],
            [ 0.,  0.,  1.,  0.],
            [ 0.,  0.,  0.,  1.]],

           [[-1.,  0.,  0.,  0.],
            [ 0., -1.,  0.,  0.],
            [ 0.,  0.,  1.,  0.],
            [ 0.,  0.,  0.,  1.]],

           [[-1.,  0.,  0.,  0.],
            [ 0.,  1.,  0.,  0.],
            [ 0.,  0., -1.,  0.],
            [ 0.,  0.,  0.,  1.]],

           [[ 1.,  0.,  0.,  0.],
            [ 0., -1.,  0.,  0.],
            [ 0.,  0., -1.,  0.],
            [ 0.,  0.,  0.,  1.]]])
In [16]:
import py3d
py3d.Vector4(q).as_transform()
Out[16]:
Transform([[[ 1.,  0.,  0.,  0.],
            [ 0.,  1.,  0.,  0.],
            [ 0.,  0.,  1.,  0.],
            [ 0.,  0.,  0.,  1.]],

           [[-1.,  0.,  0.,  0.],
            [ 0., -1.,  0.,  0.],
            [ 0.,  0.,  1.,  0.],
            [ 0.,  0.,  0.,  1.]],

           [[-1.,  0.,  0.,  0.],
            [ 0.,  1.,  0.,  0.],
            [ 0.,  0., -1.,  0.],
            [ 0.,  0.,  0.,  1.]],

           [[ 1.,  0.,  0.,  0.],
            [ 0., -1.,  0.,  0.],
            [ 0.,  0., -1.,  0.],
            [ 0.,  0.,  0.,  1.]]])

Convert quaternion to angle axis

In [18]:
import py3d
quat=py3d.Transform.from_quaternion([0.18257419, 0.36514837, 0.54772256, 0.73029674])
angle,axis=quat.as_angle_axis()
print(angle, axis)

1.504080184466401 [0.26726125 0.53452248 0.80178373]
In [19]:
py3d.Transform.from_angle_axis(angle,axis).as_quaternion()
Out[19]:
Vector4([0.18257419, 0.36514837, 0.54772256, 0.73029674])

Convert RPY to quaternion¶

Return quaternion with sequence "xyzw"

In [21]:
import py3d
rpy = [
    [0, 0, -py3d.pi],
    [0, 0, -py3d.pi-0.01],
    [0, 0, -py3d.pi/3],
    [-1, 0, 0],
    [-py3d.pi, 0, 1],
    [0, -py3d.pi, 0]]
py3d.Transform.from_rpy(rpy).as_quaternion()
Out[21]:
Vector4([[ 0.        ,  0.        , -1.        ,  0.        ],
         [ 0.        ,  0.        ,  0.9999875 ,  0.00499998],
         [ 0.        ,  0.        , -0.5       ,  0.8660254 ],
         [-0.47942554,  0.        ,  0.        ,  0.87758256],
         [-0.87758256,  0.47942554,  0.        ,  0.        ],
         [ 0.        , -1.        ,  0.        ,  0.        ]])
In [23]:
import py3d
# positions
p = [[3.24544068, -1.4610586,  1.21331756],
     [3.66378017, -1.32072563,  1.89712674],
     [-5.0884622, -2.48626808,  1.68773464],
     [-5.47338134, -2.65634697,  2.02463642],
     [1.830746, -0.8155359,  1.90245186],
     [-0.7094184, -0.84719837,  1.4467056],
     [-1.72178753, -0.681502,  1.17706321],
     [-3.88463547, -1.20610078,  1.14303617],
     [-4.527405, -3.12150274,  0.94426914],
     [4.13260871, -1.71061771,  1.49295544],
     [3.25896384, -1.46451182,  0.9032174],
     [-3.63891521, -1.03317465,  1.11405222]]
# quaternions
q = [[0.00307048, -0.27852711, -0.24115858,  0.92965357],
     [0.00955487, -0.328664, -0.25972646,  0.90798174],
     [0.05519327,  0.22308439,  0.22751421,  0.94626864],
     [0.05064761,  0.24596963,  0.23314524,  0.93945572],
     [-0.01006987, -0.17448035, -0.11591101,  0.97776267],
     [0.01230182, -0.03710485,  0.00123949,  0.99923489],
     [0.02991609,  0.0383105,  0.0729396,  0.99615117],
     [0.05252438,  0.12527874,  0.1242716,  0.98290538],
     [-0.09333274,  0.14651227,  0.2808575,  0.94389735],
     [0.00967634, -0.29085732, -0.28211318,  0.91417752],
     [0.00214324, -0.25691119, -0.23230781,  0.93809655],
     [0.04813863,  0.1177435,  0.11584668,  0.98508816]]

vertice = py3d.Vector3(z=[0, 1]) @ py3d.Transform.from_quaternion(q) @ py3d.Transform.from_translation(p)
directions = vertice.as_linesegment()
directions.start.color = py3d.Color(b=1)
directions.end.color = py3d.Color(r=1)
v = py3d.Viewer()
v.render(directions, py3d.Utils.Grid())
v.show()
Out[23]:
py3d

↑Top

←Transform

←Home